from launch import LaunchDescription
from launch.actions import ExecuteProcess, IncludeLaunchDescription, TimerAction, RegisterEventHandler, LogInfo
from launch.event_handlers import OnProcessExit, OnProcessIO
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
from launch.launch_description_sources import PythonLaunchDescriptionSource
import os

def generate_launch_description():
    # UR机械臂控制节点
    ur_control_launch = ExecuteProcess(
        cmd=['agiros', 'launch', 'ur_robotiq_sim_gz', 'ur_robotiq_sim_control.launch.py',
             'use_fake_hardware:=false', 'ur_type:=ur5', 'robot_ip:=192.168.0.101', 'launch_rviz:=false',
             'world_file:=aruco_box.sdf','description_file:=ur_robotiq_camera.urdf.xacro'
             ],
        output='screen'
    )

    # MoveIt节点
    moveit_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            os.path.join(get_package_share_directory('ur_robotiq_sim_gz'),
                        'launch/ur_robotiq_moveit.launch.py')
        ]),
        launch_arguments={
            'use_fake_hardware': 'false',
            'ur_type': 'ur5',
            'launch_rviz': 'true',
            'ur_robotiq_description_file': 'ur_robotiq_camera.urdf.xacro',
            'moveit_config_file': 'ur_robotiq_camera.srdf.xacro'
        }.items()
    )
    
    static_tf = Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            name='static_transform_publisher',
            arguments=[
        '--frame-id', 'camera_link',
        '--child-frame-id', 'rgbd_camera_link',
        '--x', '0', '--y', '0', '--z', '0',
        '--roll', '0', '--pitch', '1.57', '--yaw', '0'
            ],
            output='screen'
    )

    # Aruco识别节点
    aruco_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            os.path.join(get_package_share_directory('ur5_moveit_demo'),
                        'launch/single.launch.py')
        ]),
        launch_arguments={
            'marker_id': '582',
            'marker_size': '0.05',
            'camera_frame': 'rgbd_camera_link',
            'camera_info_topic': '/rgbd_camera/camera_info',
            'image_topic': '/rgbd_camera/image',
            'marker_frame': 'aruco_marker_frame',
            'reference_frame': 'rgbd_camera_link',
            'corner_refinement': 'SUBPIX'
        }.items()
    )

    # RQT可视化节点
    rqt_process = ExecuteProcess(
        cmd=['rqt', '--perspective-file', 
             os.path.join(get_package_share_directory('ur_robotiq_sim_gz'),
                         'config/aruco.perspective')],
        output='screen'
    )
    
# LaunchDescription中仅包含ur_control_launch和事件处理器
    return LaunchDescription([
        ur_control_launch,
        TimerAction(
            period=15.0,  # 立即执行
            actions=[static_tf, moveit_launch, aruco_launch, rqt_process]
        )
    ])
